#!/usr/bin/env python3
import rospy
from uuv_gazebo_ros_plugins_msgs.srv import SetFloat
from nav_msgs.msg import Odometry

def set_density(density):
    rospy.wait_for_service('/rexrov/set_fluid_density')
    try:
        set_density_srv = rospy.ServiceProxy('/rexrov/set_fluid_density', SetFloat)
        set_density_srv(density)
    except rospy.ServiceException as e:
        print("Service call failed: %s" % e)

def set_damping(scale):
    rospy.wait_for_service('/rexrov/set_damping_scaling')
    try:
        set_damping_srv = rospy.ServiceProxy('/rexrov/set_damping_scaling', SetFloat)
        set_damping_srv(scale)
    except rospy.ServiceException as e:
        print("Service call failed: %s" % e)

def pose_callback(msg):
    z = msg.pose.pose.position.z
    if z > 0:  # 空气中
        set_density(1.2)
        set_damping(0.01)
    else:      # 水中
        set_density(1028.0)
        set_damping(1.0)

if __name__ == "__main__":
    rospy.init_node('env_switcher')
    rospy.Subscriber('/rexrov/pose_gt', Odometry, pose_callback)
    rospy.spin()